home *** CD-ROM | disk | FTP | other *** search
/ AI Game Programming Wisdom / AIGameProgrammingWisdom.iso / SourceCode / 04 Pathfinding and Movement / 04 Pinter / Listing1.cpp next >
Encoding:
Text File  |  2001-12-09  |  2.1 KB  |  64 lines
  1. /* Copyright (C) Marco Pinter, 2001. 
  2.  * All rights reserved worldwide.
  3.  *
  4.  * This software is provided "as is" without express or implied
  5.  * warranties. You may freely copy and compile this source into
  6.  * applications you distribute provided that the copyright text
  7.  * below is included in the resulting source code, for example:
  8.  * "Portions Copyright (C) Marco Pinter, 2001"
  9.  */
  10.  
  11. // Determining the fastest route between two waypoints.
  12.  
  13. // CalculateSegment:  Determine the fastest route between two waypoints
  14. // Input parameters:  origin, orientStart, bTurnRight, radius
  15. // Output parameters: P, Q, curveLength, d, angleStart, orientFinal
  16.  
  17. // Calculate location of P (distance r from Origin, and
  18. //   center of the turning circle)
  19. // (Turning right from the origin means that P is located
  20. //   90 degrees clockwise from the initial orientation; 
  21. //   Turning left means the reverse.)
  22.  
  23. if (bTurnRight)
  24.     angleToP = orientStart - PI / 2.0;
  25. else
  26.     angleToP = orientStart + PI / 2.0;
  27.  
  28. P.x = origin.x + radius * cos(angleToP);
  29. P.y = origin.y + radius * sin(angleToP);
  30.  
  31. // Calculate distance from P to Destination.
  32. dx = dest.x - P.x;
  33. dy = dest.y - P.y;
  34. h = sqrt(dx*dx + dy*dy);
  35.  
  36. // If the destination is inside the circle,
  37. //   we can't reach it.
  38. if (h < r)
  39.    return false;
  40.  
  41. // Calculate the length of 'd' based on the other two sides
  42. //   of the right triangle, and theta as well.
  43. d = sqrt(h*h - radius*radius);
  44. theta = acos(radius / h);
  45.  
  46. // Calculate angle phi from arctangent relationship
  47. phi = atan2(dy, dx);
  48.  
  49. // Determine point Q from position P and angle
  50. angleFinal = (bTurnRight ? phi + theta : phi - theta);
  51. Q.x = P.x + radius * cos(angleFinal);
  52. Q.y = P.y + radius * sin(angleFinal);
  53.  
  54. // Calculate final values needed:
  55. //  Total distance of curve; and final orientation
  56. angleStart = angleToP + PI_VAL / 2;
  57. totalCurve = bTurnRight ? angleStart - angleFinal
  58.                         : angleFinal - angleStart;
  59. curveLength = totalCurve * radius;
  60. orientFinal = bTurnRight ? angleFinal - PI_VAL/2 
  61.                          : angleFinal + PI_VAL/2;
  62.  
  63. return true;
  64.